#include "robot.h"

using namespace std;

Robot::Robot()
{
    position = QPoint();
    orientation = 0.0;
    vWheel[0] = 0.0;
    vWheel[1] = 0.0;
    vWheel[2] = 0.0;
    vWheel[3] = 0.0;
    spin = false;
    kickX = 0.0;
    kickZ = 0.0;
    this->connect = NULL;
    this->id = 0;
}

Robot::Robot(connection& connect, QPoint pos, double Orient, int id)
{
    position = pos;
    orientation = Orient;
    vWheel[0] = 0.0;
    vWheel[1] = 0.0;
    vWheel[2] = 0.0;
    vWheel[3] = 0.0;
    spin = false;
    kickX = 0.0;
    kickZ = 0.0;
    this->connect = &connect;
    this->id = id;
}

QPoint Robot::getPosition()
{
    return position;
}

double Robot::getVelocityWheel(int wheel)
{
    return vWheel[wheel];
}

double Robot::getOrientation()
{
    return orientation;
}

void Robot::setPosition(QPoint pos)
{
    position = pos;
}

void Robot::setOrientation(double orientation)
{
    this->orientation = orientation;
}

void Robot::velocityTransformation(double vx, double vy, double w)
{
    vWheel[0] = -vx/2 - sqrt(3)*vy/2 + w/4;
    vWheel[1] = sqrt(2)*vx/2 - sqrt(2)*vy/2 + w/4;
    vWheel[2] = (sqrt(2)*vx/2) + sqrt(2)*vy/2 + (w/4);
    vWheel[3] = -vx/2 + sqrt(3)*vy/2 + w/4;
}

void Robot::sendConditions()
{
    connect->setid(id);
    connect->setWheel1(vWheel[0]);
    connect->setWheel2(vWheel[1]);
    connect->setWheel3(vWheel[2]);
    connect->setWheel4(vWheel[3]);
    connect->setKickSpeedx(kickX);
    connect->setteam( true );
    connect->setspin(spin);
    connect->send();
}

void Robot::followTheBall(QPoint ball)
{
    double vx= 0;//(x_ball - x_robot)/100;
    double vy= 0;//(y_ball - y_robot)/100;
    double w;
    spin = true;


    if( abs((int)(ball.x()-position.x()))< 150 && abs((int)(ball.y() - position.y())) < 150  ){


        vx = 0;
        vy = 0;
        w = 0;

    }
    else {
        double Tp = (atan2(ball.y() - position.y(),ball.x() - position.x()));
    double dT = (orientation - Tp)-3.1415/2;

    vy = -10*sin(dT) * abs((int)(ball.y() - position.y())) * abs((int)(ball.x() - position.x()))/16000;
    vx = 10*cos(dT) * abs((int)(ball.y() - position.y())) * abs((int)(ball.x() - position.x()))/16000;

    if(vx > 60) vx = 60;
    else if(vx < -60) vx = -60;
    if(vy > 60) vy = 60;
    else if(vy < -60) vy = -60;


    w = 0.3*(Tp - orientation)*180/3.1415;

    cout <<  abs((int)(ball.y() - position.y())) * abs((int)(ball.x() - position.x()))/16000 << endl;
    cout << (abs((int)(ball.x()-position.x()))/80) << endl;

    }

    this->velocityTransformation(vx, vy, w);

    this->sendConditions();
}

void Robot::shoot(double vX, double vZ)
{

}

void Robot::goalKeeper()
{

}

void Robot::passTo(Robot& robot)
{

}

void Robot::twist(double newOrientation)
{

}
